package de.hhn.mim.psb;

import java.util.ArrayList;
import java.util.List;

import Jama.EigenvalueDecomposition;
import Jama.Matrix;

/**
 * Rumpf-Klasse zur Implementierung einer Landmarken-basierten 
 * Registrierung. 
 * Die zwei Methoden calcTrafo und TRE muessen implementiert werden
 * Zur Berechnung des TRE muessen zusaetzlich Teile der Klasse PCA3D 
 * implementiert werden.
 * 
 * Die untenstehenden Kommentare und die Methodenbeschreibungen
 * erlaeutern die zu implementierende Funktionalitaet
 * 
 * Implementieren Sie zuerst die Methode calcTrafo, am Ende des Moduls
 * 
 * 
 * Diese Klasse kann eine Landmarken basierte Registrierung durchfuehren
 * Die Methode calcTrafo erwartet 3 Paare von korrespondierenden Landmarken
 * und liefert eine Matrix, mit der die Landmarken "b" auf die Position der
 * Landmarken "a" abgebildet werden koennen. 
 * 
 * Bei der Berechnung der Transformation werden nur Translationen und 
 * Rotationen aber keine Skalierungen beruecksichtigt. Von daher wird i.d.R. 
 * keine Transformation gefunden, welche die Punkte exakt aufeinander abbildet.
 * Je nach dem, in welcher Reihenfolge die Punkte bei der Berechnung der
 * Transformationen beruecksichtigt werden, ergeben sich unterschiedliche
 * Transformationen (Loesung ist bei 3 Punktpaaren ueberbestimmt.
 * 
 * Die Methode calcTrafos erwartet ebenfalls 3 Paare von Landmarken und
 * ruft calcTrafo mit allen möglichen Permutationen in der Reihenfolge der
 * Punkte auf, um so alle möglichen Transformationen zu berechnen.
 * Sie berechnet zusaetzlich den Mittelwert aller Transformationen
 * 
 * Eine weitere Methode calcTrafos bekommt ein beliebig langes Array von
 * Landmarken und berechnet alle Loesungen und auch eine mittlere 
 * Transformation
 * 
 * die Methode calcFRE bestimmt fuer eine gegebene Transformation den
 * mittleren Fiducial Registration Error
 * 
 * Die Methode calcTRE bestimmt fuer eine gegebene Transformation und einen
 * gegebenen Zielpunkt den Target Registration Error
 * 
 * @author Rolf Bendl
 * 
 * 2012-05-12: Klasse erstellt
 * 2013-01-20: Kommentar zu Methode calcTrafo korrigiert 
 * Die Gesamttransformation ergibt sich dann aus der Multiplikation der 
 * Transformation B und der inversen Transformation A
 * Zuvor stand dort "ergibt sich aus der Summe". Transformationsmatrizen
 * mit homogenen Koord. werden nie addiert, sondern immer multipliziert
 * 2014-02-05: Kommentare und prints ergaenzt in calcTRE
 * 
 */

public class LMRegist
{
	public static class LMPair
	{
		Vect3D first;
		Vect3D second;
		public LMPair(Vect3D first, Vect3D second)
		{
			this.first  = first;
			this.second = second;
		}
	}

	private static LMPair[] lms;

	/**
	 * 
	 */
	public LMRegist()
	{
	}
	/**
	 * Schaetzt den Target Registration Error (TRE), fuer den uebergenen 
	 * Zielpunkt (target) der sich bei einer Transformation (trafo) ergibt,
	 * die auf Basis von Landmarken (points) berechnet wurde.
	 * Zuvor muss der Fiducial Registration Error berechnet und dieser
	 * Methode im Parameter fre uebergeben werden.
	 * 
	 * Zur Berechnung des TRE ist eine Hauptkomponentenanalyse notwendig
	 * Dazu kann die Klasse PCA3D genutzt werden.
	 * 
	 * Berechnung erfolgt nach der Formel im Skript
	 * Optische Trackingsysteme (Eisenmann)
	 * aus dem Bachelorschwerpunkt Diagnose- und Therapiesysteme 
	 * (Folie 46, SS 2012):
	 *    
	 * TRE^2(r) ~ FRE^2/(N-2) (1 + 1/3 * (dx^2/fx^2 + dy^2/fy^2 +dz^2/fz^2) 
	 * mit d Abstand Zielpunkt von Hauptachse k
	 * mit f RMS-Abstand der Fiducials von Hauptachse
	 * @param target der Zielpunkt
	 * @param fre der bereits berechnete fre
	 * @param points die Punkte
	 * @param trafo die Transformation
	 * @return der TRE
	 * 
	 * Anm.: wenn die Methode als target den Schwerpunkt von N=3 uebergebenen 
	 * Punkten erhaelt, (Schwerpunkt = Zentrum der Hauptachsen), 
	 * ist das Ergebnis = FRE da dx = dy = dz = 0 ist
	 */
	public static double calcTRE(Vect3D target, double fre ,
			Vect3D [] points, Matrix3D trafo)
	{
		int        print   = 1;
		PCA3D       pca    = new PCA3D(points);
		double [][] eVects = pca.getEigenVectors();
		double   [] eVals  = pca.getEigenValues();
		double dx=0.0;
		double dy=0.0;
		double dz=0.0;
		Vect3D test = trafo.mult(target);

		// Spalten sind die Eigenvektoren 
		// TODO Implementierung fehlt - Ich habe einfach mal rumversucht
		
		double TRE2 = 0;
		double tre  = Math.sqrt(TRE2);

		if(print> 0) System.out.printf("TRE:    %5.2f\n", tre);
		
		double sum = 0.0;

		Vect3D meanFiducial = Vect3D.rms(points);
		
		for(int i=0;i<3;i++){
			double d = LineToPointDistance2D(new double[]{0,0,0},
												new double[]{eVects[0][i],eVects[1][i],eVects[2][i]},
											    new double[]{target.x,target.y,target.z},false);
			double f = LineToPointDistance2D(new double[]{0,0,0},
											new double[]{eVects[0][i],eVects[1][i],eVects[2][i]},
										    new double[]{meanFiducial.x,meanFiducial.y,meanFiducial.z},false);
			sum+= Math.pow(d, 2)/Math.pow(f, 2);
		}
		
		tre=Math.sqrt(fre/(points.length-2)*(1+(sum/3)));

		return tre;
	}

	//Compute the cross product AB x AC
	private static double CrossProduct(double[] pointA, double[] pointB, double[] pointC)
	{
	    double[] AB = new double[2];
	    double[] AC = new double[2];
	    AB[0] = pointB[0] - pointA[0];
	    AB[1] = pointB[1] - pointA[1];
	    AC[0] = pointC[0] - pointA[0];
	    AC[1] = pointC[1] - pointA[1];
	    double cross = AB[0] * AC[1] - AB[1] * AC[0];

	    return cross;
	}
	
	//Compute the dot product AB . AC
	private static double DotProduct(double[] pointA, double[] pointB, double[] pointC)
	{
	    double[] AB = new double[2];
	    double[] BC = new double[2];
	    AB[0] = pointB[0] - pointA[0];
	    AB[1] = pointB[1] - pointA[1];
	    BC[0] = pointC[0] - pointB[0];
	    BC[1] = pointC[1] - pointB[1];
	    double dot = AB[0] * BC[0] + AB[1] * BC[1];

	    return dot;
	}
	
	//Compute the distance from A to B
	static double  Distance(double[] pointA, double[] pointB)
	{
	    double d1 = pointA[0] - pointB[0];
	    double d2 = pointA[1] - pointB[1];

	    return Math.sqrt(d1 * d1 + d2 * d2);
	}

	//Compute the distance from AB to C
	//if isSegment is true, AB is a segment, not a line.
	public static double LineToPointDistance2D(double[] pointA, double[] pointB, double[] pointC, 
							boolean isSegment)
	{
		
	    double dist = CrossProduct(pointA, pointB, pointC) / Distance(pointA, pointB);
	    if (isSegment)
	    {
	        double dot1 = DotProduct(pointA, pointB, pointC);
	        if (dot1 > 0) 
	            return Distance(pointB, pointC);

	        double dot2 = DotProduct(pointB, pointA, pointC);
	        if (dot2 > 0) 
	            return Distance(pointA, pointC);
	    }
	    return Math.abs(dist);
	} 
	
	
	
	public static double vectDist(Vect3D a,Vect3D b){
		return Math.sqrt(Math.pow(a.x-b.x,2)+Math.pow(a.y-b.y,2)+Math.pow(a.z-b.z,2));
	}

	
	/**
	 * berechnet den Fiducial Registration Error (FRE) fuer ein Array von
	 * korrespondierenden Landmarken auf Basis der uebergebenen Transformation
	 * @param lms die Landmarken 
	 * @param trafo die Transformation
	 * @return der FRE
	 */
	public static double calcFRE(LMPair [] lms, Matrix3D trafo)
	{
		int    print    = 1;
		double fre2     = 0;
		double min_diff = Double.MAX_VALUE;
		double max_diff = Double.MIN_VALUE;

		if(print > 0) System.out.println("LMRegist::calcFRE:");
		for(int i = 0; i < lms.length; i++)
		{
			Vect3D trans = trafo.mult(lms[i].second);
			Vect3D diff  = trans.subtractVect(lms[i].first);
			double norm  = diff.norm();
			double norm2 = norm * norm;
			fre2 = fre2 + norm2;
			if(norm < min_diff) min_diff = norm;
			if(norm > max_diff) max_diff = norm;
			if(print > 1) 
				System.out.printf("lms[%d] norm %f min %f \n",i,norm, min_diff);
		}
		fre2 = fre2 / lms.length;
		double fre = Math.sqrt(fre2);
		if(print > 0) 
			System.out.printf("FRE2 %7.2f min_dev %7.2f max_dev %7.2f FRE %7.2f\n", 
					fre2, min_diff, max_diff, fre);
		return fre;
	}


	/**
	 * Erhaelt ein Array mit korrespondierenden Landmarkenpaaren.
	 * Berechnet die Transformationen mit der die Punkte LMPair[i].second auf 
	 * die Punkte LMPair[i].first abgebildet werden koennen
	 *
	 * Mit drei Stuetzpunkten ist die Transformation bereits ueberbestimmt
	 * Wenn nicht alle Punkte exakt aufeinander abgebildet werden koennen
	 * (z.B. aufgrund mangelnder Praezision bei der interaktiven Bestimmung
	 * der Landmarken), ergeben sich unterschiedliche Transformation,
	 * je nach dem, in welcher Reihenfolge die zu transformierenden Punkte 
	 * auf ihre Korrespondenzen abgebildet werden.
	 * Bei 4 und mehr Landmarkenpaaren ergeben sich zusaetzliche 
	 * Transformationen, wenn die Landmarkenpaare unterschiedlich kombiniert 
	 * werden. n Landmarkenpaare koennen in n ueber 3 unterschiedliche 
	 * Kombinationen aufgeteilt werden.
	 *
	 * Die Methode liefert eine Liste von Transformationen, die sich aufgrund
	 * der verschiedenen Kombinationsm�glichkeiten ergeben.
	 * Die letzte Transformation ist der mittlere Transformation, 
	 * (Mittelung der Matrixelemente ueber alle Transformationen) 
	 * 
	 * @param lms die Landmarkenpaare
	 * @return Liste mit Transformationen
	 */

	public static ArrayList<Matrix3D> calcTrafos(LMPair [] lms)
	{
		int                 print  = 0;
		ArrayList<Matrix3D> trafos = new ArrayList<Matrix3D>();
		int no_of = lms.length;
		if(no_of == 3)
		{
			Matrix3D [] myTrafos;
			myTrafos = calcTrafos(lms[0].first, lms[1].first, lms[2].first, 
					lms[0].second, lms[1].second, lms[2].second);  
			// myTrafos[length - 1] liefert die durchschnittliche Trafo aller anderen
			// 6 Transformationsmoeglichkeiten, wird nicht in die Menge der 
			// Transformationen uebernommen, da unten nochmal MW ueber alle 
			// Trafos berechnet wird
			for(int m = 0; m < (myTrafos.length - 1); m++) 
				trafos.add(myTrafos[m]);
		}
		else if(no_of > 3)
		{
			for(int i = 0; i < no_of - 2; i++)
			{
				for(int j = i+1; j < no_of - 1; j++)
				{
					for(int k = j+1; k < no_of; k++)
					{
						Matrix3D [] myTrafos;
						myTrafos = calcTrafos(lms[i].first, lms[j].first, lms[k].first, 
								lms[i].second, lms[j].second, lms[k].second);
						for(int m = 0; m < (myTrafos.length - 1); m++)
							trafos.add(myTrafos[m]);
					}
				}
			}
		}    
		Matrix3D mean = new Matrix3D(0);
		for(int i = 0; i < trafos.size(); i++)
		{
			mean = mean.add(trafos.get(i));
		}
		mean = mean.mult(1.0/trafos.size());
		trafos.add(mean);
		return trafos;
	}

	/**
	 * erhaelt zweimal drei Punkte aX und bX. Berechnet die Transformationen
	 * mit der die Punkte bx auf die Punkte ax abgebildet werden koennen
	 * 
	 * Mit drei Stuetzpunkten ist die Transformation bereits ueberbestimmt
	 * Wenn nicht alle Punkte exakt aufeinander abgebildet werden koennen
	 * (z.B. aufgrund mangelnder Praezision bei der interaktiven Bestimmung
	 * der Landmarken), ergeben sich unterschiedliche Transformation,
	 * je nach dem, in welcher Reihenfolge die Punkte bX auf ihre Korrespondenzen
	 * aX abgebildet werden.
	 * Es werden Transformationen fuer alle moeglichen Reihenfolgen (6) berechnet
	 * und als Array von Transformationsmatrizen zurueck geliefert
	 * In einer siebten Transformationsmatrix wird die mittlere Transformation
	 * (Mittellung der Matrixkoeffizienten aller 6 zuvor berechneten) zurueck 
	 * gegeben.
	 * @param aX: Zielkoordinaten
	 * @param bX: zu transformierende Koordinaten
	 * @return Array von Transformationsmatrizen
	 */
	public static Matrix3D [] calcTrafos(Vect3D a1, Vect3D a2, Vect3D a3, 
			Vect3D b1, Vect3D b2, Vect3D b3)
	{
		Matrix3D [] trafos = new Matrix3D [7];
		int         print  = 1;
		int         print2 = 2;
		trafos[0] = calcTrafo(a1, a2, a3, b1, b2, b3);
		trafos[1] = calcTrafo(a2, a3, a1, b2, b3, b1);
		trafos[2] = calcTrafo(a3, a1, a2, b3, b1, b2);
		trafos[3] = calcTrafo(a1, a3, a2, b1, b3, b2);
		trafos[4] = calcTrafo(a2, a1, a3, b2, b1, b3);
		trafos[5] = calcTrafo(a3, a2, a1, b3, b2, b1);
		trafos[6] = new Matrix3D(0);
		for(int i = 0; i < 6; i++)
			trafos[6] = trafos[6].add(trafos[i]);
		trafos[6] = trafos[6].mult(1.0/6.0);

		if(print > 0) System.out.println("LMRegist::calcTrafos:");
		if(print2 > 0) Vect3D.print("a    ", a1, a2, a3);
		if(print2 > 0) Vect3D.print("b    ", b1, b2, b3);
		for(int i = 0; i < 7; i++)
		{
			if(print2 > 2) trafos[i].print("Trafo[" + i + "]");
			Vect3D tb1 = trafos[i].mult(b1);
			Vect3D tb2 = trafos[i].mult(b2);
			Vect3D tb3 = trafos[i].mult(b3);
			if(print2 > 1) Vect3D.print("tb[" + i +"]", tb1, tb2, tb3);      
		}
		return trafos;
	}

	/**
	 * erhaelt zweimal drei Punkte aX und bX. Berechnet die Transformation
	 * mit der die Punkte bx auf die Punkte ax abgebildet werden koennen
	 * und gibt die Transformationsmatrix zurueck
	 * @param aX: Zielkoordinaten
	 * @param bX: zu transformierende Koordinaten
	 * @return Transformationsmatrix
	 */

	public static Matrix3D calcTrafo(Vect3D a1, Vect3D a2, Vect3D a3, 
			Vect3D b1, Vect3D b2, Vect3D b3)
	{
		int print = 0;
		return calcTrafo(a1,a2,a3,b1,b2,b3,print);
	}
	/**
	 * erhaelt zweimal drei Punkte aX und bX. Berechnet die Transformation
	 * mit der die Punkte bx auf die Punkte ax abgebildet werden koennen
	 * und gibt die Transformationsmatrix zurueck
	 * @param aX: Zielkoordinaten
	 * @param bX: zu transformierende Koordinaten
	 * @param print: wenn print > 0 werden die einzelnen berechneten 
	 * Transformationsmatrizen in der Console ausgegeben
	 * Wenn print > 1, werden erweiterete Informationen ausgegeben
	 * (Punktkoordinaten, Distanzen zw. transformierten Punkten b und a),
	 * berechnete Winkel). wenn print > 2 werden die Koordinaten von
	 * aX und bX ausgegeben, nach dem sie in die gemeinsame xz-Ebene 
	 * abgebildet wurden.
	 * @return Transformationsmatrix
	 * 
	 * Die Berechnung kann analog der Folien im Skript 
	 * Diagnose-Therapie-Visualisierung-Registrierung (SS 2012), 
	 * Komplexe Transformationen (Folie 32ff).
	 * Dort ist dargestellt, welche Transformation der Reihe nach notwendig sind
	 * um drei Punkte so zu transformieren, dass einer von ihnen im Ursprung,
	 * der zweite auf der x-Achse und der dritte in der xz-Ebene landet.
	 * *
	 * Wenn die Transformation gesucht wird, die drei Punkte Ax auf drei beliebige
	 * Punkte Bx abbildet (unter der Voraussetzung dass die r�umlichen Beziehungen
	 * zwischen den Punkten A und den Punkten B gleich sind), kann man die Gesamt-
	 * Transformation dadurch berechnen, dass man erst die Punkte Ax auf
	 * Ursprung, x-Achse und xz-Ebene abbildet und dann fuer die Punkte Bx das
	 * gleiche macht. 
	 * Die Gesamttransformation ergibt sich dann aus der Multiplikation der 
	 * Transformation B und der inversen Transformation A
	 * 
	 * Hinweis: Es empfiehlt sich, zur Berechnung der Rotationen normierte
	 * Koordinaten zu verwenden 
	 * 
	 */
	public static Matrix3D calcTrafo(Vect3D a1, Vect3D a2, Vect3D a3, 
			Vect3D b1, Vect3D b2, Vect3D b3,
			int print) {

		Matrix3D A = trafoStep(a1, a2, a3);	  
		Matrix m = new Matrix(A.v);
		A.print("A");
		m = m.inverse();
		A.v = m.getArray();
		A.print("A-1");
		Matrix3D B = trafoStep(b1, b2, b3);
		return B.mult(A);
	}    
	
	private static Matrix3D trafoStep(Vect3D a1,Vect3D a2,Vect3D a3){	
		
		//Abbildung der a-Punkt auf Ursprung, x-Achse und xz-Ebene
		//Verschieben der Punkte, so dass a1 im Ursprung liegt   
		Vect3D translToZero = new Vect3D(-a1.x,-a1.y,-a1.z);
		Matrix3D T = Matrix3D.translation(translToZero);
		T.print();
		
		Vect3D a1ToZero = T.mult(a1);
		Vect3D a2ToZero = T.mult(a2);
		Vect3D a3ToZero = T.mult(a3);

		//Normalisieren
		a1 = a1.normalize();
		a2 = a2.normalize();
		a3 = a3.normalize();
		
		//Rotation der Linie zwischen a1-a2 in die xz-Ebene
		double d1 = Math.sqrt(Math.pow(a2ToZero.x,2)+Math.pow(a2ToZero.y,2));
		double phiZ = Math.atan2(a2ToZero.y,a2ToZero.x);		
		System.out.println("(XZ) Rot by: "+Math.toDegrees(phiZ));		
		Matrix3D Rz = Matrix3D.rotationZ(-phiZ);
		Vect3D a1InXZ = Rz.mult(a1ToZero);    
		Vect3D a2InXZ = Rz.mult(a2ToZero);
		Vect3D a3InXZ = Rz.mult(a3ToZero);

		//Rotation, so dass a1-a2 in Richtung x-Achse zeigt
		double d2 = Math.sqrt(Math.pow(a2.y-a1.y,2)+Math.pow(a2.z-a1.z,2)+Math.pow(a2.x-a1.x,2)); 		
		double phiY = Math.atan2(a2InXZ.z,a2InXZ.x);
		System.out.println("(In X) Rot by: "+Math.toDegrees(phiY));
		Matrix3D Ry = Matrix3D.rotationY(phiY);
		Vect3D a1InXDir = Ry.mult(a1InXZ);
		Vect3D a2InXDir = Ry.mult(a2InXZ);
		Vect3D a3InXDir = Ry.mult(a3InXZ);

		//Rotation, so dass a1-a3 in xz-Ebene liegt
		double d3 = Math.sqrt(Math.pow(a3InXDir.y,2)+Math.pow(a3InXDir.z, 2));		
		double phiX = Math.atan2(a3InXDir.y, a3InXDir.z);
		System.out.println("(In XZ) Rot by: "+Math.toDegrees(phiX));
		Matrix3D Rx = Matrix3D.rotationX(phiX);
		Vect3D a1InXZPlane = Rx.mult(a1InXDir);
		Vect3D a2InXZPlane = Rx.mult(a2InXDir);
		Vect3D a3InXZPlane = Rx.mult(a3InXDir);		
		
		Matrix3D R = Matrix3D.translation(new Vect3D());
		//TODO: noch zusammenfassen				
		R = Ry.mult(Rx);
		R = Rz.mult(R);
		R = T.mult(R);
		
		//FIXME: Test, ob diese Matrix tatsaechlich die korrekte Transformation ausfuehrt
		Vect3D a1CompleteTrans = R.mult(a1);
		Vect3D a2CompleteTrans = R.mult(a2);
		Vect3D a3CompleteTrans = R.mult(a3);
		
		if(!(a1CompleteTrans.equals(a1InXZPlane)&&
			 a2CompleteTrans.equals(a2InXZPlane)&&
			  a3CompleteTrans.equals(a3InXZPlane))){
			System.err.println("Transformation nicht ok!");
		}
		
		return R;
	}
	//angle = atan2(norm(cross(a,b)), dot(a,b))

	private static double calcAngle(Vect3D a, Vect3D b){
		double c = a.crossProduct(b).norm();
		double d = a.dotProduct(b);
		return Math.atan2(c, d);
	}
	
	private static double atan2Sw(double y,double x){
		double theta_rad = Math.atan2(y,x);
		return (theta_rad/Math.PI*180) + (theta_rad > 0 ? 0 : 360);
	}

}
